% orig code: https://zhuanlan.zhihu.com/p/471457420

clear all;
close all;
%控制顶点
P = [9, 27, 37;
    5, 17, 50 ];
P=P';

figure(1);
%在图上标出控制点，A,B,C
plot(P(:,1),P(:,2),'o','LineWidth',1,'MarkerEdgeColor','k',...
    'MarkerFaceColor','g','MarkerSize',6);
line(P(:,1),P(:,2));
for i=1:size(P,1)
    text(P(i,1)+2,P(i,2),char(65+i-1));
end
%轴标题
xlabel('X','FontSize',20);
ylabel('Y','FontSize',20);
xlim([0 max(P(:,1))+5]);
ylim([0 max(P(:,2))+5]);

%拟合D点位置
hold on;
C1=Bezier_1st(P(1:2,:),0.3);
text(C1(1,1)+2,C1(1,2),char(65+i));
%拟合E点位置
C2=Bezier_1st(P(2:3,:),0.3);
text(C2(1,1)+2,C2(1,2),char(65+i+1));
line([C1(1,1) C2(1,1)],[C1(1,2) C2(1,2)]);

%拟合F点位置
C3=Bezier_1st([C1(1,:);C2(1,:)],0.3);
text(C3(1,1)+2,C3(1,2),char(65+i+2));

%降次迭代，拟合F点轨迹
for t=0.001:0.001:1
    Cd=Bezier_1st([P(1:2,:)],t,1);
    Ce=Bezier_1st([P(2:3,:)],t,1);
    C(int16(t/0.001),:)=Bezier_1st([Cd;Ce],t,1);
end

plot(C(:,1),C(:,2),'cyan-','LineWidth',1);
line(C(:,1),C(:,2));
hold on